Strict
Import mojo
Import agfx

Const S_TIMESTEP:Float = 0.1
#Rem
Header: VERLET Physic - WORLD manager
#End


#Rem
Summary: Collision info returned from DetectCollision
#End
Class TVerletCollisionInfo

		#Rem
		Summary: imparct length
		#End
		Field depth:Float
		#Rem
		Summary: collision link normal
		#End
		Field normal:TVector2
		#Rem
		Summary: collision link
		#End
		Field link:TVerletLink
		#Rem
		Summary: collision point
		#End
		Field point:TVerletPoint
		
		#Rem
		Summary: Constructor
		#End		
		Method New()
			Self.depth = 0.0
			Self.normal = New TVector2()
		End Method
End Class



#Rem
Summary: Verlet physics world class
#End
Class TVerletWorld

	#Rem
	Summary: Verlet physics Groups container
	#End
	Field groups:ArrayList<TVerletGroup>
	
	#Rem
	Summary: world gravity
	#End
	Field gravity:TVector2 = New TVector2(0,0)
	
	#Rem
	Summary: world range definition, all groups are locked inside
	#End
	Field range:TBBox

	#Rem
	Summary:  constructor
	#End
	Method New()
		Self.groups = New ArrayList<TVerletGroup>()
	End

	#Rem
	Summary: Set gravity
	#End
	Method SetGravity:Void(g:TVector2)
		Self.gravity = g
	End

	#Rem
	Summary: Set world range
	#End
	Method SetRange:Void(c:TVector2,w:Float,h:Float)

		Self.range = New TBBox()
		Self.range.Set(c,New TVector2(w,h),w/2.0)
		
	End


	#Rem
	Summary: Add verlet group to list
	#End
	Method AddGroup:Void(g:TVerletGroup)
		Self.groups.AddLast(g)
	End

	#Rem
	Summary: Update world
	Add this call to your Upfate loop
	#End
	Method Update:Void()
		Local g:TVerletGroup
		
		For g = EachIn Self.groups
		
			If g.active Then 
			
				For Local l:TVerletLink = EachIn g.links
					l.Update()
				Next
				
				For Local p:TVerletPoint = EachIn g.points					
					p.SetGravity(Self.gravity)								
					p.Update()
				Next
				
				g.CalcBoundingBox()
				g.CalcCenterPoint()

				Self.IterateCollisionsWith(g)	
				Self.IterateRangeCollisionWith(g)
								
			EndIf
		Next
		
	End

	#Rem
	Summary: Render debug information
	you don't need it in final application	
	#End
	Method Render:Void()
		Local g:TVerletGroup
		For g = EachIn Self.groups
			g.Render()
		Next
	End

	#Rem
	Summary: Detect collision between two verlet groups
	return collision information
	#End
	Method DetectCollision:TVerletCollisionInfo( B1 : TVerletGroup, B2 : TVerletGroup )

		Local minDistance:Float = 10000.0
		Local collisionInfo:TVerletCollisionInfo = New TVerletCollisionInfo()
		
		For Local i:Int = 0 To  B1.links.Size() + B2.links.Size() - 1

			Local E : TVerletLink

			If( i < B1.links.Size())

				E = B1.links.Get( i )
			Else
				E = B2.links.Get( i - B1.links.Size() )
			Endif
			
			' Calculate the axis perpendicular to this edge and normalize it
			
			Local Axis : TVector2 = New TVector2( E.p1.pos.y,  E.p2.pos.x )
			Axis.x = Axis.x - E.p2.pos.y
			Axis.y = Axis.y - E.p1.pos.x
			Axis = VecNormalize(Axis)


			'Project both bodies onto the perpendicular

			Local MinA : Float[ 1 ]
			Local MinB : Float[ 1 ]
			Local MaxA : Float[ 1 ]
			Local MaxB : Float[ 1 ]

			
			B1.ProjectToAxis( Axis, MinA, MaxA )
			B2.ProjectToAxis( Axis, MinB, MaxB )


			'Calculate the distance between the two intervals

			Local distance : Float = Self.IntervalDistance( MinA, MaxA, MinB, MaxB )


			'If the intervals don't overlap, return, since there is no collision

			If distance > 0.0

				Return null

			ElseIf Abs( distance ) < minDistance

				minDistance = Abs( distance )
				collisionInfo.normal = Axis
				collisionInfo.link      = E 
			Endif

		Next
		
		collisionInfo.depth = minDistance
		
		'Ensure that the body containing the collision edge lies in B2 And the one conatining the collision vertex in B1
		If( collisionInfo.link.parent <> B2 )

			Local Temp:TVerletGroup = B2
			B2 = B1
			B1 = Temp
		Endif

		'This is needed To make sure that the collision normal is pointing at B1
		Local SignVec : TVector2 = New TVector2( collisionInfo.normal)
		Local CenterDiff : TVector2 = New TVector2( B1.center)
		CenterDiff.Sub( B2.center )
		Local SignFloat : Float = SignVec.x * CenterDiff.x + SignVec.y * CenterDiff.y
		Local Sign : Int = Sgn( SignFloat )
				
		
		'Remember that the line equation is N*( R - R0 ). We choose B2->Center as R0; the normal N is given by the collision normal
		If Sign <> 1
			'Revert the collision normal If it points away from B1
			collisionInfo.normal.Mult(-1.0)
		Endif

		'Initialize the smallest distance To a large value

		Local SmallestD:Float = 10000.0

		Local Distance:Float


		For Local i:Int = 0 To B1.points.Size() - 1
		
			'Measure the distance of the vertex from the line using the line equation	
			Local cDist : TVector2 = New TVector2( B1.points.Get(i).pos)
			cDist.Sub( B2.center )

			Distance = collisionInfo.normal.x * cDist.x + collisionInfo.normal.y * cDist.y
			
			'If the measured distance is smaller than the smallest distance reported so far, set the smallest distance And the collision vertex

			If Distance < SmallestD

				SmallestD = Distance

				collisionInfo.point = B1.points.Get(i)

			Endif

		Next
		
		Return collisionInfo

	End Method
	
	#Rem
	Summary: Help function for calculate distance between bboxes of groups
	#End
	Method IntervalDistance:Float(MinA:Float[], MaxA:Float[], MinB:Float[], MaxB:Float[])
	
		Local res:Float
		
		If MinA[ 0 ] < MinB[ 0 ]
			Return MinB[ 0 ] - MaxA[ 0 ]
		Else
			Return MinA[ 0 ] - MaxB[ 0 ]
		Endif
		
		Return res	
	End Method
	
	#Rem
	Summary: Check if groups are overlaped
	#End
	Method BodiesOverlap : Bool( B1 : TVerletGroup, B2 : TVerletGroup )
			Return ( B1.MinX <= B2.MaxX ) And ( B1.MinY <= B2.MaxY ) And ( B1.MaxX >= B2.MinX ) And ( B2.MaxY >= B1.MinY )
	End Method
	
	#Rem
	Summary: apply collision result to element in collision
	#End
	Method ProcessCollision : Void(collisionInfo:TVerletCollisionInfo)
	
		Local E1 : TVerletPoint = collisionInfo.link.p1
		Local E2 : TVerletPoint = collisionInfo.link.p2
			
		Local CollisionVector : TVector2 = New TVector2( collisionInfo.normal.x * collisionInfo.depth, collisionInfo.normal.y * collisionInfo.depth )
	
		Local T : Float

		If Abs( E1.pos.x - E2.pos.x ) > Abs( E1.pos.y - E2.pos.y )
		
			T = ( collisionInfo.point.pos.x - CollisionVector.x - E1.pos.x ) / (  E2.pos.x - E1.pos.x )
		Else
			T = ( collisionInfo.point.pos.y - CollisionVector.y - E1.pos.y ) / (  E2.pos.y - E1.pos.y )
		Endif
		
		Local Lambda : Float = 1.0 / ( T * T + ( 1.0 - T ) * ( 1.0 - T ) )

		If E1.active Then
			E1.pos.x -= CollisionVector.x * ( 1.0 - T ) * 0.5 * Lambda
			E1.pos.y -= CollisionVector.y * ( 1.0 - T ) * 0.5 * Lambda
		Else
			E1.pos.Set(E1.old_pos)
		EndIf
	
		If E2.active Then
			E2.pos.x -= CollisionVector.x * T * 0.5 * Lambda
			E2.pos.y -= CollisionVector.y * T * 0.5 * Lambda
		Else
			E2.pos.Set(E2.old_pos)
		EndIf
		
		collisionInfo.point.pos.x = collisionInfo.point.pos.x + CollisionVector.x * 0.5
		collisionInfo.point.pos.y = collisionInfo.point.pos.y + CollisionVector.y * 0.5

	End Method
	
	
	#Rem
	Summary: check collision between all groups
	#End
	Method IterateCollisions : Void()
		
				
		'Iterate trough all bodies
		Local B1 : Int
		Local B2 : Int
		Local gB1 : TVerletGroup
		Local gB2 : TVerletGroup
		Local coll:TVerletCollisionInfo
		
		For B1 = 0 To Self.groups.Size()- 1
			For B2 = 0 To Self.groups.Size()- 1			
				If B1 <> B2
					gB1 = Self.groups.Get(B1)
					gB2 = Self.groups.Get(B2)
					
					'test bounding boxes
					If Self.BodiesOverlap( gB1,gB2 )
					
						'if there is a collision respond to it
						coll = Self.DetectCollision( gB1,gB2  )
						If coll
							Self.ProcessCollision(coll)
						Endif
					Endif
				Endif
			Next
		Next
			
	End Method
	
	
	#Rem
	Summary: check collision between group with all others
	#End
	Method IterateCollisionsWith : Void(gB1 : TVerletGroup)
		
				
		'Iterate trough all bodies
		Local B1 : Int
		Local B2 : Int
		
		Local gB2 : TVerletGroup
		Local coll:TVerletCollisionInfo
		
		For B2 = 0 To Self.groups.Size()- 1			
			gB2 = Self.groups.Get(B2)
			If gB1 <> gB2					
									
				'test bounding boxes
				If Self.BodiesOverlap( gB1,gB2 )
				
					'if there is a collision respond to it
					coll = Self.DetectCollision( gB1,gB2  )
					If coll
						Self.ProcessCollision(coll)
					Endif
				Endif
			Endif
		Next
			
	End Method
	
	#Rem
	Summary: Check and iterate range collision for all groups from list
	#End
	Method IterateRangeCollision:Void()
		
		Local g: TVerletGroup		
		Local p: TVerletPoint

		For g = EachIn  Self.groups			
			g.IterateRangeCollision(Self.range)	
		Next
	End
	
	#Rem
	Summary: Check and iterate range collision for group
	#End
	Method IterateRangeCollisionWith:Void(g: TVerletGroup)		
		g.IterateRangeCollision(Self.range)		
	End

End Class

